#!/usr/bin/env python
# -*-coding:utf-8 -*-

__all__ = ['RobotPool', '__init__', 'add', 'robotExistInTheWay']

class RobotPool(object):
    def __init__(self):
        """
        创建一个障碍物池。
        """
        self._robots = []
        
    def getRobots(self):
        """
        获取所有机器人的一个列表。
        :return: 所有机器人的一个列表。
        """
        return self._robots
        
    def getRobotsNum(self):
        """
        获取机器人的数量。
        :return: 机器人的数量。
        """
        return len(self._robots)
        
    def getRobot(self, idx):
        """
        获取下标为idx的机器人。
        :return: 下标为idx的机器人。
        """
        return self._robots[idx]
        
    def add(self, robot):
        """
        往障碍物池中添加一个机器人。
        :param obstacle: 待添加的障碍物（Obstacle对象）。
        """
        self._robots.append(robot)
        
    def robotExistInTheWay(self, robot):
        """
        判断在（机器人的）行进道路上是否存在其它机器人。
        :param robot: 待判断是否在行进道路上存在其它机器人的机器人。
        :return: True如果存在其它机器人，否则返回False。
        """
        position = robot.getPos()
        velocity = robot.getVel()
        detection_range = robot.getDetectionRange()
        detection_angle  = robot.getDetectionAngle()
        for r in self._robots:
            if r is robot:
                continue
            vec = r.getPos()-position
            if vec.norm() < detection_range and \
               vec.angleWith(velocity) < 0.5 * detection_angle:
                return True
        return False